{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "!curl -L https://raw.githubusercontent.com/facebookresearch/habitat-sim/main/examples/colab_utils/colab_install.sh | NIGHTLY=true bash -s"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%cd /content/habitat-sim\n",
    "## [setup]\n",
    "import os\n",
    "import random\n",
    "import sys\n",
    "\n",
    "import git\n",
    "import magnum as mn\n",
    "import numpy as np\n",
    "\n",
    "import habitat_sim\n",
    "from habitat_sim.utils import viz_utils as vut\n",
    "\n",
    "if \"google.colab\" in sys.modules:\n",
    "    os.environ[\"IMAGEIO_FFMPEG_EXE\"] = \"/usr/bin/ffmpeg\"\n",
    "\n",
    "repo = git.Repo(\".\", search_parent_directories=True)\n",
    "dir_path = repo.working_tree_dir\n",
    "%cd $dir_path\n",
    "data_path = os.path.join(dir_path, \"data\")\n",
    "output_path = os.path.join(\n",
    "    dir_path, \"examples/tutorials/managed_rigid_object_tutorial_output/\"\n",
    ")\n",
    "\n",
    "\n",
    "def place_agent(sim):\n",
    "    # place our agent in the scene\n",
    "    agent_state = habitat_sim.AgentState()\n",
    "    agent_state.position = [-0.15, -0.7, 1.0]\n",
    "    agent_state.rotation = np.quaternion(-0.83147, 0, 0.55557, 0)\n",
    "    agent = sim.initialize_agent(0, agent_state)\n",
    "    return agent.scene_node.transformation_matrix()\n",
    "\n",
    "\n",
    "def make_configuration():\n",
    "    # simulator configuration\n",
    "    backend_cfg = habitat_sim.SimulatorConfiguration()\n",
    "    backend_cfg.scene_id = os.path.join(\n",
    "        data_path, \"scene_datasets/habitat-test-scenes/apartment_1.glb\"\n",
    "    )\n",
    "    assert os.path.exists(backend_cfg.scene_id)\n",
    "    backend_cfg.enable_physics = True\n",
    "\n",
    "    # sensor configurations\n",
    "    # Note: all sensors must have the same resolution\n",
    "    # setup 2 rgb sensors for 1st and 3rd person views\n",
    "    camera_resolution = [544, 720]\n",
    "    sensor_specs = []\n",
    "\n",
    "    rgba_camera_1stperson_spec = habitat_sim.CameraSensorSpec()\n",
    "    rgba_camera_1stperson_spec.uuid = \"rgba_camera_1stperson\"\n",
    "    rgba_camera_1stperson_spec.sensor_type = habitat_sim.SensorType.COLOR\n",
    "    rgba_camera_1stperson_spec.resolution = camera_resolution\n",
    "    rgba_camera_1stperson_spec.position = [0.0, 0.6, 0.0]\n",
    "    rgba_camera_1stperson_spec.orientation = [0.0, 0.0, 0.0]\n",
    "    rgba_camera_1stperson_spec.sensor_subtype = habitat_sim.SensorSubType.PINHOLE\n",
    "    sensor_specs.append(rgba_camera_1stperson_spec)\n",
    "\n",
    "    depth_camera_1stperson_spec = habitat_sim.CameraSensorSpec()\n",
    "    depth_camera_1stperson_spec.uuid = \"depth_camera_1stperson\"\n",
    "    depth_camera_1stperson_spec.sensor_type = habitat_sim.SensorType.DEPTH\n",
    "    depth_camera_1stperson_spec.resolution = camera_resolution\n",
    "    depth_camera_1stperson_spec.position = [0.0, 0.6, 0.0]\n",
    "    depth_camera_1stperson_spec.orientation = [0.0, 0.0, 0.0]\n",
    "    depth_camera_1stperson_spec.sensor_subtype = habitat_sim.SensorSubType.PINHOLE\n",
    "    sensor_specs.append(depth_camera_1stperson_spec)\n",
    "\n",
    "    rgba_camera_3rdperson_spec = habitat_sim.CameraSensorSpec()\n",
    "    rgba_camera_3rdperson_spec.uuid = \"rgba_camera_3rdperson\"\n",
    "    rgba_camera_3rdperson_spec.sensor_type = habitat_sim.SensorType.COLOR\n",
    "    rgba_camera_3rdperson_spec.resolution = camera_resolution\n",
    "    rgba_camera_3rdperson_spec.position = [0.0, 1.0, 0.3]\n",
    "    rgba_camera_3rdperson_spec.orientation = [-45, 0.0, 0.0]\n",
    "    rgba_camera_3rdperson_spec.sensor_subtype = habitat_sim.SensorSubType.PINHOLE\n",
    "    sensor_specs.append(rgba_camera_3rdperson_spec)\n",
    "\n",
    "    # agent configuration\n",
    "    agent_cfg = habitat_sim.agent.AgentConfiguration()\n",
    "    agent_cfg.sensor_specifications = sensor_specs\n",
    "\n",
    "    return habitat_sim.Configuration(backend_cfg, [agent_cfg])\n",
    "\n",
    "\n",
    "def simulate(sim, dt=1.0, get_frames=True):\n",
    "    # simulate dt seconds at 60Hz to the nearest fixed timestep\n",
    "    print(\"Simulating \" + str(dt) + \" world seconds.\")\n",
    "    observations = []\n",
    "    start_time = sim.get_world_time()\n",
    "    while sim.get_world_time() < start_time + dt:\n",
    "        sim.step_physics(1.0 / 60.0)\n",
    "        if get_frames:\n",
    "            observations.append(sim.get_sensor_observations())\n",
    "\n",
    "    return observations\n",
    "\n",
    "\n",
    "# [/setup]\n",
    "if __name__ == \"__main__\":\n",
    "    import argparse\n",
    "\n",
    "    parser = argparse.ArgumentParser()\n",
    "    parser.add_argument(\"--no-show-video\", dest=\"show_video\", action=\"store_false\")\n",
    "    parser.add_argument(\"--no-make-video\", dest=\"make_video\", action=\"store_false\")\n",
    "    parser.set_defaults(show_video=True, make_video=True)\n",
    "    args, _ = parser.parse_known_args()\n",
    "    show_video = args.show_video\n",
    "    make_video = args.make_video\n",
    "    if make_video and not os.path.exists(output_path):\n",
    "        os.mkdir(output_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "    # [initialize]\n",
    "    # create the simulators AND resets the simulator\n",
    "\n",
    "    cfg = make_configuration()\n",
    "    try:  # Got to make initialization idiot proof\n",
    "        sim.close()\n",
    "    except NameError:\n",
    "        pass\n",
    "    sim = habitat_sim.Simulator(cfg)\n",
    "    agent_transform = place_agent(sim)\n",
    "\n",
    "    # get the primitive assets attributes manager\n",
    "    prim_templates_mgr = sim.get_asset_template_manager()\n",
    "\n",
    "    # get the physics object attributes manager\n",
    "    obj_templates_mgr = sim.get_object_template_manager()\n",
    "\n",
    "    # get the rigid object manager\n",
    "    rigid_obj_mgr = sim.get_rigid_object_manager()\n",
    "    # [/initialize]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [basics]\n",
    "\n",
    "    # load some object templates from configuration files\n",
    "    sphere_template_id = obj_templates_mgr.load_configs(\n",
    "        str(os.path.join(data_path, \"test_assets/objects/sphere\"))\n",
    "    )[0]\n",
    "\n",
    "    # add a sphere to the scene, returns the object\n",
    "    sphere_obj = rigid_obj_mgr.add_object_by_template_id(sphere_template_id)\n",
    "    # move sphere\n",
    "    sphere_obj.translation = [2.50, 0.0, 0.2]\n",
    "\n",
    "    # simulate\n",
    "    observations = simulate(sim, dt=1.5, get_frames=make_video)\n",
    "\n",
    "    if make_video:\n",
    "        vut.make_video(\n",
    "            observations,\n",
    "            \"rgba_camera_1stperson\",\n",
    "            \"color\",\n",
    "            output_path + \"sim_basics\",\n",
    "            open_vid=show_video,\n",
    "        )\n",
    "\n",
    "    # [/basics]\n",
    "    rigid_obj_mgr.remove_all_objects()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "    # [object_user_configurations]\n",
    "    # modify an object's user-defined configurations\n",
    "\n",
    "    # load some object templates from configuration files\n",
    "    sphere_template_id = obj_templates_mgr.load_configs(\n",
    "        str(os.path.join(data_path, \"test_assets/objects/sphere\"))\n",
    "    )[0]\n",
    "\n",
    "    # add a sphere to the scene, returns the object\n",
    "    sphere_obj = rigid_obj_mgr.add_object_by_template_id(sphere_template_id)\n",
    "\n",
    "    # set user-defined configuration values\n",
    "    user_attributes_dict = {\n",
    "        \"obj_val_0\": \"This is a sphere object named \" + sphere_obj.handle,\n",
    "        \"obj_val_1\": 17,\n",
    "        \"obj_val_2\": False,\n",
    "        \"obj_val_3\": 19.7,\n",
    "        \"obj_val_4\": [2.50, 0.0, 0.2],\n",
    "        \"obj_val_5\": mn.Quaternion.rotation(mn.Deg(90.0), [-1.0, 0.0, 0.0]),\n",
    "    }\n",
    "    for k, v in user_attributes_dict.items():\n",
    "        sphere_obj.user_attributes.set(k, v)\n",
    "\n",
    "    for k, _ in user_attributes_dict.items():\n",
    "        print(\n",
    "            \"Sphere Object user attribute : {} : {}\".format(\n",
    "                k, sphere_obj.user_attributes.get_as_string(k)\n",
    "            )\n",
    "        )\n",
    "\n",
    "    # [/object_user_configurations]\n",
    "    rigid_obj_mgr.remove_all_objects()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [dynamic_control]\n",
    "\n",
    "    observations = []\n",
    "    obj_templates_mgr.load_configs(\n",
    "        str(os.path.join(data_path, \"objects/example_objects/\"))\n",
    "    )\n",
    "    # search for an object template by key sub-string\n",
    "    cheezit_template_handle = obj_templates_mgr.get_template_handles(\n",
    "        \"data/objects/example_objects/cheezit\"\n",
    "    )[0]\n",
    "    # build multiple object initial positions\n",
    "    box_positions = [\n",
    "        [2.39, -0.37, 0.0],\n",
    "        [2.39, -0.64, 0.0],\n",
    "        [2.39, -0.91, 0.0],\n",
    "        [2.39, -0.64, -0.22],\n",
    "        [2.39, -0.64, 0.22],\n",
    "    ]\n",
    "    box_orientation = mn.Quaternion.rotation(mn.Deg(90.0), [-1.0, 0.0, 0.0])\n",
    "    # instance and place the boxes\n",
    "    boxes = []\n",
    "    for b in range(len(box_positions)):\n",
    "        boxes.append(\n",
    "            rigid_obj_mgr.add_object_by_template_handle(cheezit_template_handle)\n",
    "        )\n",
    "        boxes[b].translation = box_positions[b]\n",
    "        boxes[b].rotation = box_orientation\n",
    "\n",
    "    # anti-gravity force f=m(-g) using first object's mass (all objects have the same mass)\n",
    "    anti_grav_force = -1.0 * sim.get_gravity() * boxes[0].mass\n",
    "\n",
    "    # throw a sphere at the boxes from the agent position\n",
    "    sphere_template = obj_templates_mgr.get_template_by_id(sphere_template_id)\n",
    "    sphere_template.scale = [0.5, 0.5, 0.5]\n",
    "\n",
    "    obj_templates_mgr.register_template(sphere_template)\n",
    "\n",
    "    # create sphere\n",
    "    sphere_obj = rigid_obj_mgr.add_object_by_template_id(sphere_template_id)\n",
    "\n",
    "    sphere_obj.translation = sim.agents[0].get_state().position + [0.0, 1.0, 0.0]\n",
    "    # get the vector from the sphere to a box\n",
    "    target_direction = boxes[0].translation - sphere_obj.translation\n",
    "    # apply an initial velocity for one step\n",
    "    sphere_obj.linear_velocity = target_direction * 5\n",
    "    sphere_obj.angular_velocity = [0.0, -1.0, 0.0]\n",
    "\n",
    "    start_time = sim.get_world_time()\n",
    "    dt = 3.0\n",
    "    while sim.get_world_time() < start_time + dt:\n",
    "        # set forces/torques before stepping the world\n",
    "        for box in boxes:\n",
    "            box.apply_force(anti_grav_force, [0.0, 0.0, 0.0])\n",
    "            box.apply_torque([0.0, 0.01, 0.0])\n",
    "        sim.step_physics(1.0 / 60.0)\n",
    "        observations.append(sim.get_sensor_observations())\n",
    "\n",
    "    if make_video:\n",
    "        vut.make_video(\n",
    "            observations,\n",
    "            \"rgba_camera_1stperson\",\n",
    "            \"color\",\n",
    "            output_path + \"dynamic_control\",\n",
    "            open_vid=show_video,\n",
    "        )\n",
    "\n",
    "    # [/dynamic_control]\n",
    "    rigid_obj_mgr.remove_all_objects()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [kinematic_interactions]\n",
    "\n",
    "    chefcan_template_handle = obj_templates_mgr.get_template_handles(\n",
    "        \"data/objects/example_objects/chefcan\"\n",
    "    )[0]\n",
    "    chefcan_obj = rigid_obj_mgr.add_object_by_template_handle(chefcan_template_handle)\n",
    "    chefcan_obj.translation = [2.4, -0.64, 0.0]\n",
    "    # set object to kinematic\n",
    "    chefcan_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC\n",
    "\n",
    "    # drop some dynamic objects\n",
    "    chefcan_obj_2 = rigid_obj_mgr.add_object_by_template_handle(chefcan_template_handle)\n",
    "    chefcan_obj_2.translation = [2.4, -0.64, 0.28]\n",
    "    chefcan_obj_3 = rigid_obj_mgr.add_object_by_template_handle(chefcan_template_handle)\n",
    "    chefcan_obj_3.translation = [2.4, -0.64, -0.28]\n",
    "    chefcan_obj_4 = rigid_obj_mgr.add_object_by_template_handle(chefcan_template_handle)\n",
    "    chefcan_obj_4.translation = [2.4, -0.3, 0.0]\n",
    "\n",
    "    # simulate\n",
    "    observations = simulate(sim, dt=1.5, get_frames=True)\n",
    "\n",
    "    if make_video:\n",
    "        vut.make_video(\n",
    "            observations,\n",
    "            \"rgba_camera_1stperson\",\n",
    "            \"color\",\n",
    "            output_path + \"kinematic_interactions\",\n",
    "            open_vid=show_video,\n",
    "        )\n",
    "\n",
    "    # [/kinematic_interactions]\n",
    "\n",
    "    rigid_obj_mgr.remove_all_objects()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [kinematic_update]\n",
    "    observations = []\n",
    "\n",
    "    clamp_template_handle = obj_templates_mgr.get_template_handles(\n",
    "        \"data/objects/example_objects/largeclamp\"\n",
    "    )[0]\n",
    "    clamp_obj = rigid_obj_mgr.add_object_by_template_handle(clamp_template_handle)\n",
    "    clamp_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC\n",
    "    clamp_obj.translation = [0.8, 0.2, 0.5]\n",
    "\n",
    "    start_time = sim.get_world_time()\n",
    "    dt = 1.0\n",
    "    while sim.get_world_time() < start_time + dt:\n",
    "        # manually control the object's kinematic state\n",
    "        clamp_obj.translation += [0.0, 0.0, 0.01]\n",
    "        clamp_obj.rotation = (\n",
    "            mn.Quaternion.rotation(mn.Rad(0.05), [-1.0, 0.0, 0.0]) * clamp_obj.rotation\n",
    "        )\n",
    "        sim.step_physics(1.0 / 60.0)\n",
    "        observations.append(sim.get_sensor_observations())\n",
    "\n",
    "    if make_video:\n",
    "        vut.make_video(\n",
    "            observations,\n",
    "            \"rgba_camera_1stperson\",\n",
    "            \"color\",\n",
    "            output_path + \"kinematic_update\",\n",
    "            open_vid=show_video,\n",
    "        )\n",
    "\n",
    "    # [/kinematic_update]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [velocity_control]\n",
    "\n",
    "    # get object VelocityControl structure and setup control\n",
    "    vel_control = clamp_obj.velocity_control\n",
    "    vel_control.linear_velocity = [0.0, 0.0, -1.0]\n",
    "    vel_control.angular_velocity = [4.0, 0.0, 0.0]\n",
    "    vel_control.controlling_lin_vel = True\n",
    "    vel_control.controlling_ang_vel = True\n",
    "\n",
    "    observations = simulate(sim, dt=1.0, get_frames=True)\n",
    "\n",
    "    # reverse linear direction\n",
    "    vel_control.linear_velocity = [0.0, 0.0, 1.0]\n",
    "\n",
    "    observations += simulate(sim, dt=1.0, get_frames=True)\n",
    "\n",
    "    if make_video:\n",
    "        vut.make_video(\n",
    "            observations,\n",
    "            \"rgba_camera_1stperson\",\n",
    "            \"color\",\n",
    "            output_path + \"velocity_control\",\n",
    "            open_vid=show_video,\n",
    "        )\n",
    "\n",
    "    # [/velocity_control]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [local_velocity_control]\n",
    "\n",
    "    vel_control.linear_velocity = [0.0, 0.0, 2.3]\n",
    "    vel_control.angular_velocity = [-4.3, 0.0, 0.0]\n",
    "    vel_control.lin_vel_is_local = True\n",
    "    vel_control.ang_vel_is_local = True\n",
    "\n",
    "    observations = simulate(sim, dt=1.5, get_frames=True)\n",
    "\n",
    "    # video rendering\n",
    "    if make_video:\n",
    "        vut.make_video(\n",
    "            observations,\n",
    "            \"rgba_camera_1stperson\",\n",
    "            \"color\",\n",
    "            output_path + \"local_velocity_control\",\n",
    "            open_vid=show_video,\n",
    "        )\n",
    "\n",
    "    # [/local_velocity_control]\n",
    "    rigid_obj_mgr.remove_all_objects()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "    # [embodied_agent]\n",
    "\n",
    "    # load the lobot_merged asset\n",
    "    locobot_template_id = obj_templates_mgr.load_configs(\n",
    "        str(os.path.join(data_path, \"objects/locobot_merged\"))\n",
    "    )[0]\n",
    "\n",
    "    # add robot object to the scene with the agent/camera SceneNode attached\n",
    "    locobot = rigid_obj_mgr.add_object_by_template_id(\n",
    "        locobot_template_id, sim.agents[0].scene_node\n",
    "    )\n",
    "    locobot.translation = [1.75, -1.02, 0.4]\n",
    "\n",
    "    vel_control = locobot.velocity_control\n",
    "    vel_control.linear_velocity = [0.0, 0.0, -1.0]\n",
    "    vel_control.angular_velocity = [0.0, 2.0, 0.0]\n",
    "\n",
    "    # simulate robot dropping into place\n",
    "    observations = simulate(sim, dt=1.5, get_frames=make_video)\n",
    "\n",
    "    vel_control.controlling_lin_vel = True\n",
    "    vel_control.controlling_ang_vel = True\n",
    "    vel_control.lin_vel_is_local = True\n",
    "    vel_control.ang_vel_is_local = True\n",
    "\n",
    "    # simulate forward and turn\n",
    "    observations += simulate(sim, dt=1.0, get_frames=make_video)\n",
    "\n",
    "    vel_control.controlling_lin_vel = False\n",
    "    vel_control.angular_velocity = [0.0, 1.0, 0.0]\n",
    "\n",
    "    # simulate turn only\n",
    "    observations += simulate(sim, dt=1.5, get_frames=make_video)\n",
    "\n",
    "    vel_control.angular_velocity = [0.0, 0.0, 0.0]\n",
    "    vel_control.controlling_lin_vel = True\n",
    "    vel_control.controlling_ang_vel = True\n",
    "\n",
    "    # simulate forward only with damped angular velocity (reset angular velocity to 0 after each step)\n",
    "    observations += simulate(sim, dt=1.0, get_frames=make_video)\n",
    "\n",
    "    vel_control.angular_velocity = [0.0, -1.25, 0.0]\n",
    "\n",
    "    # simulate forward and turn\n",
    "    observations += simulate(sim, dt=2.0, get_frames=make_video)\n",
    "\n",
    "    vel_control.controlling_ang_vel = False\n",
    "    vel_control.controlling_lin_vel = False\n",
    "\n",
    "    # simulate settling\n",
    "    observations += simulate(sim, dt=3.0, get_frames=make_video)\n",
    "\n",
    "    # remove the agent's body while preserving the SceneNode\n",
    "    rigid_obj_mgr.remove_object_by_id(locobot.object_id, delete_object_node=False)\n",
    "\n",
    "    # demonstrate that the locobot object does not now exist'\n",
    "    print(\"Locobot is still alive : {}\".format(locobot.is_alive))\n",
    "\n",
    "    # video rendering with embedded 1st person view\n",
    "    if make_video:\n",
    "        vut.make_video(\n",
    "            observations,\n",
    "            \"rgba_camera_1stperson\",\n",
    "            \"color\",\n",
    "            output_path + \"robot_control\",\n",
    "            open_vid=show_video,\n",
    "        )\n",
    "\n",
    "    # [/embodied_agent]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "    # [embodied_agent_navmesh]\n",
    "\n",
    "    # load the lobot_merged asset\n",
    "    locobot_template_id = obj_templates_mgr.load_configs(\n",
    "        str(os.path.join(data_path, \"objects/locobot_merged\"))\n",
    "    )[0]\n",
    "    # add robot object to the scene with the agent/camera SceneNode attached\n",
    "    locobot = rigid_obj_mgr.add_object_by_template_id(\n",
    "        locobot_template_id, sim.agents[0].scene_node\n",
    "    )\n",
    "    initial_rotation = locobot.rotation\n",
    "\n",
    "    # set the agent's body to kinematic since we will be updating position manually\n",
    "    locobot.motion_type = habitat_sim.physics.MotionType.KINEMATIC\n",
    "\n",
    "    # create and configure a new VelocityControl structure\n",
    "    # Note: this is NOT the object's VelocityControl, so it will not be consumed automatically in sim.step_physics\n",
    "    vel_control = habitat_sim.physics.VelocityControl()\n",
    "    vel_control.controlling_lin_vel = True\n",
    "    vel_control.lin_vel_is_local = True\n",
    "    vel_control.controlling_ang_vel = True\n",
    "    vel_control.ang_vel_is_local = True\n",
    "    vel_control.linear_velocity = [0.0, 0.0, -1.0]\n",
    "\n",
    "    # try 2 variations of the control experiment\n",
    "    for iteration in range(2):\n",
    "        # reset observations and robot state\n",
    "        observations = []\n",
    "        locobot.translation = [1.75, -1.02, 0.4]\n",
    "        locobot.rotation = initial_rotation\n",
    "        vel_control.angular_velocity = [0.0, 0.0, 0.0]\n",
    "\n",
    "        video_prefix = \"robot_control_sliding\"\n",
    "        # turn sliding off for the 2nd pass\n",
    "        if iteration == 1:\n",
    "            sim.config.sim_cfg.allow_sliding = False\n",
    "            video_prefix = \"robot_control_no_sliding\"\n",
    "\n",
    "        # manually control the object's kinematic state via velocity integration\n",
    "        start_time = sim.get_world_time()\n",
    "        last_velocity_set = 0\n",
    "        dt = 6.0\n",
    "        time_step = 1.0 / 60.0\n",
    "        while sim.get_world_time() < start_time + dt:\n",
    "            previous_rigid_state = locobot.rigid_state\n",
    "\n",
    "            # manually integrate the rigid state\n",
    "            target_rigid_state = vel_control.integrate_transform(\n",
    "                time_step, previous_rigid_state\n",
    "            )\n",
    "\n",
    "            # snap rigid state to navmesh and set state to object/agent\n",
    "            end_pos = sim.step_filter(\n",
    "                previous_rigid_state.translation, target_rigid_state.translation\n",
    "            )\n",
    "            locobot.translation = end_pos\n",
    "            locobot.rotation = target_rigid_state.rotation\n",
    "\n",
    "            # Check if a collision occured\n",
    "            dist_moved_before_filter = (\n",
    "                target_rigid_state.translation - previous_rigid_state.translation\n",
    "            ).dot()\n",
    "            dist_moved_after_filter = (end_pos - previous_rigid_state.translation).dot()\n",
    "\n",
    "            # NB: There are some cases where ||filter_end - end_pos|| > 0 when a\n",
    "            # collision _didn't_ happen. One such case is going up stairs.  Instead,\n",
    "            # we check to see if the the amount moved after the application of the filter\n",
    "            # is _less_ than the amount moved before the application of the filter\n",
    "            EPS = 1e-5\n",
    "            collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter\n",
    "\n",
    "            # run any dynamics simulation\n",
    "            sim.step_physics(time_step)\n",
    "\n",
    "            # render observation\n",
    "            observations.append(sim.get_sensor_observations())\n",
    "\n",
    "            # randomize angular velocity\n",
    "            last_velocity_set += time_step\n",
    "            if last_velocity_set >= 1.0:\n",
    "                vel_control.angular_velocity = [0.0, (random.random() - 0.5) * 2.0, 0.0]\n",
    "                last_velocity_set = 0\n",
    "\n",
    "        # video rendering with embedded 1st person views\n",
    "        if make_video:\n",
    "            sensor_dims = (\n",
    "                sim.get_agent(0).agent_config.sensor_specifications[0].resolution\n",
    "            )\n",
    "            overlay_dims = (int(sensor_dims[1] / 4), int(sensor_dims[0] / 4))\n",
    "            overlay_settings = [\n",
    "                {\n",
    "                    \"obs\": \"rgba_camera_1stperson\",\n",
    "                    \"type\": \"color\",\n",
    "                    \"dims\": overlay_dims,\n",
    "                    \"pos\": (10, 10),\n",
    "                    \"border\": 2,\n",
    "                },\n",
    "                {\n",
    "                    \"obs\": \"depth_camera_1stperson\",\n",
    "                    \"type\": \"depth\",\n",
    "                    \"dims\": overlay_dims,\n",
    "                    \"pos\": (10, 30 + overlay_dims[1]),\n",
    "                    \"border\": 2,\n",
    "                },\n",
    "            ]\n",
    "\n",
    "            vut.make_video(\n",
    "                observations=observations,\n",
    "                primary_obs=\"rgba_camera_3rdperson\",\n",
    "                primary_obs_type=\"color\",\n",
    "                video_file=output_path + video_prefix,\n",
    "                fps=60,\n",
    "                open_vid=show_video,\n",
    "                overlay_settings=overlay_settings,\n",
    "                depth_clip=10.0,\n",
    "            )\n",
    "\n",
    "    # [/embodied_agent_navmesh]"
   ]
  }
 ],
 "metadata": {
  "accelerator": "GPU",
  "colab": {
   "name": "Managed Rigid Object Tutorial",
   "provenance": []
  },
  "jupytext": {
   "cell_metadata_filter": "-all",
   "formats": "nb_python//py:percent,colabs//ipynb",
   "main_language": "python",
   "notebook_metadata_filter": "all"
  },
  "kernelspec": {
   "display_name": "Python 3",
   "name": "python3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
